#include"ArmorDetector.h"
//#define SHOW_ROI 
//#define SHOW_BINARY 
//#define SHOW_PROCESSING 
#define SHOW_ARMOR 
#define LINERECT
#define CIRCLEARMOR
#define GETROI
//#define LINEARMOR
void adjust_rect(RotatedRect& minRect){
    if(minRect.size.height<minRect.size.width){
        float temp=minRect.size.height;
        minRect.size.height=minRect.size.width;
        minRect.size.width=temp;
    }

    return ;
}
void ArmorDetector::set_frame(Mat a){
    a.copyTo(frame);
    return ;
}
void ArmorDetector::set_mode(int m){
    enemy_color=m;
    return ;
}
bool ArmorDetector::find(){
    if(findtarget)
        return 1;
    else return 0;
}
void ArmorDetector::Pretreatment(){
    possibleRects.clear();
    Mat thres_whole,thecolor,splited[3], target_image;
    vector<vector<Point>> contours;
	vector<Vec4i> hireachy;
    Point2f vertex[4];
    ///////////////Get ROI////////////////////////
    // 注意这个_res_last是一个旋转矩形
#ifdef GETROI
    const cv::Point & last_result = currentCenter;

    // 如果上一次的目标没了，源图就是输入的图
    // 并且搜索的ROI矩形（_dect_rect）就是整个图像
    if(last_result.x == 0 || last_result.y == 0){
        ROIimage = frame;
        _dect_rect = Rect(0, 0, frame.cols, frame.rows);
        diff=Point(0,0);
    }
    else{
       
        // 如果上一次的目标没有丢失的话，用直立矩形包围上一次的旋转矩形
        Rect rect = _res_last;
        // 截图的区域大小。太大的话会把45度识别进去
        double scale_w = 1.3 + 0.7;
        double scale_h = 2;

        int w = int(rect.width * scale_w);
        int h = int(rect.height * scale_h);
        Point center = last_result;
        int x = std::max(center.x - w, 0);
        int y = std::max(center.y - h, 0);
        Point lu = Point(x, y);  /* point left up */
        x = std::min(center.x + w, frame.cols);
        y = std::min(center.y + h, frame.rows);
        Point rd = Point(x, y);  /* point right down */
     
        // 构造出矩形找到了搜索的ROI区域
        _dect_rect = Rect(lu, rd);
        diff=lu;
        // 为false说明矩形是空的，所以继续搜索全局像素
        // 感觉这里会有点bug
        // 矩形是空的则返回false
        if (makeRectSafe(_dect_rect, frame.size()) == false){
            _dect_rect = Rect(0, 0, frame.cols, frame.rows);
            ROIimage = frame;
            diff=Point(0,0);
        }
        else
            // 如果ROI矩形合法的话就用此ROI
            frame(_dect_rect).copyTo(ROIimage);
    }
#else
    frame.copyTo(ROIimage);
    diff.x=0;
    diff.y=0;
#endif

#ifdef SHOW_ROI
    imshow("ROI",ROIimage);
#endif
    /////////////START PROCESSING//////////////////
    Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3));
    cvtColor(ROIimage,thres_whole,COLOR_BGR2GRAY);
    //threshold(thres_whole,thres_whole, 200, 255, THRESH_BINARY);
    adaptiveThreshold(thres_whole,thres_whole,255,ADAPTIVE_THRESH_MEAN_C,THRESH_BINARY,7,2);
    //imshow("threshold",thres_whole);
    split(ROIimage,splited);
    if(enemy_color==1)//1==red,2==blue
        subtract(splited[2], splited[0], thecolor);
    else
        subtract(splited[0], splited[2], thecolor);
    //imshow("thecolor",thecolor);
    threshold(thecolor, thecolor,110, 255, THRESH_BINARY);
    //imshow("thecolor2",thecolor);
    dilate(thecolor, thecolor, element);
    
    target_image = thecolor & thres_whole; //imshow("processed img",target_image);//get the shining and colorful target
    dilate(target_image, target_image, element);//Prevent the light bar from being too thin.
   
    findContours(target_image, contours, hireachy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
    //1.select the possible contours with some conditions.
    for (int i = 0; i < contours.size(); ++i) {
		RotatedRect minRect = minAreaRect(contours[i]);
		minRect.points(vertex);
        
  
		bool condition_angle1 = (fabs(minRect.angle) < 15.0) && minRect.size.height > minRect.size.width; 
        bool condition_angle2 = (fabs(minRect.angle) > 75.0) && minRect.size.width > minRect.size.height; 
        adjust_rect(minRect);
        bool condition_area = (minRect.size.height*minRect.size.width) > 200 && (minRect.size.height*minRect.size.width)  < 6000;
        bool condition_ratio = (minRect.size.height/ minRect.size.width > 2.5) && (minRect.size.height/ minRect.size.width < 5.5);
        if((condition_angle1||condition_angle2)&&condition_area&&condition_ratio){
            possibleRects.push_back(minRect);//cout<<"Height: "<<minRect.size.height<<endl;
#ifdef LINERECT    
            for (int l = 0; l < 4; l++)
			    line(frame, Point(vertex[l].x+diff.x,vertex[l].y+diff.y), Point(vertex[(l + 1) % 4].x+diff.x,vertex[(l + 1) % 4].y+diff.y), Scalar(0, 250, 0), 2);
#endif        // putText(frame,to_string(minRect.angle),minRect.center,FONT_HERSHEY_COMPLEX,3,Scalar(250,0,0),2);
        }
           
	}
    
    return ;
}
void ArmorDetector::GetArmor(){
    
    findtarget=0;
    Pretreatment();
    //Matching light bars
    double max_area=0;
    int index1=0,index2=0;
    for(int i=0;i<possibleRects.size();i++){
        for(int j=i+1;j<possibleRects.size();j++){
            const RotatedRect& i_light = possibleRects[i];//左灯条
            const RotatedRect& j_light = possibleRects[j];//右灯条
             float heightDiff = fabs(i_light.size.height-j_light.size.height);

            //比对宽
            float widthDiff = fabs(i_light.size.width - j_light.size.width);

            //比对角度
            float angleDiff = fabs(i_light.angle - j_light.angle);
            float dis = sqrt((i_light.center.x - j_light.center.x) * (i_light.center.x - j_light.center.x) + (i_light.center.y - j_light.center.y) * (i_light.center.y - j_light.center.y));

            //左右灯条高度的平均值
            float meanheight = (i_light.size.height + j_light.size.height) / 2;

            //左右灯条中心点y的差值
            float yDiff = fabs(i_light.center.y - j_light.center.y);

            //y差比率
            float yDiffRatio = yDiff / meanheight;

            //左右灯条中心点x的差值
            float xDiff = fabs(i_light.center.x - j_light.center.x);

            //x差比率
            float xDiffRatio = xDiff / meanheight;

            //相距距离与灯条长度比值
            float ratio = dis / meanheight;

            //？？？
            float heightDiff_ratio = heightDiff / max(i_light.size.height, j_light.size.height);

            //Unqualified condition
            bool con_angle=angleDiff>20&angleDiff<70;
            bool con_xDiffR=xDiffRatio <1.5||xDiffRatio >4.0;
            bool con_yDiffR=yDiffRatio>0.5;
            bool con_ratio=ratio<1.5||ratio>4.0;
            bool con_area=((i_light.size.height*i_light.size.width)/(j_light.size.height*j_light.size.width)>2)&&((i_light.size.height*i_light.size.width)/(j_light.size.height*j_light.size.width)<0.6);

            if (con_angle||con_xDiffR||con_yDiffR||con_ratio||con_area)
                continue;
            else findtarget=1;

#ifdef CIRCLEARMOR
            Point2f _currentCenter;
            _currentCenter.x = (i_light.center.x + j_light.center.x) / 2+diff.x;
	        _currentCenter.y = (i_light.center.y +  j_light.center.y) / 2+diff.y;
            line(frame, Point(_currentCenter.x - 10, _currentCenter.y - 10),
			Point(_currentCenter.x + 10, _currentCenter.y + 10), Scalar(255, 255, 0), 5);
		    line(frame, Point(_currentCenter.x + 10, _currentCenter.y - 10),
			Point(_currentCenter.x - 10, _currentCenter.y + 10), Scalar(255, 255, 0), 5);
		    circle(frame, _currentCenter, 7.5, Scalar(0, 0, 255), 5);
#endif
            if(meanheight*xDiff >max_area)
            {
                max_area=xDiff*meanheight;
                index1=i;
                index2=j;
            }
        }
    }
    if(!findtarget){
        currentCenter.x=0;
        currentCenter.y=0;

        return;
    }
    currentCenter.x = (possibleRects[index1].center.x + possibleRects[index2].center.x) / 2+diff.x;
	currentCenter.y = (possibleRects[index1].center.y + possibleRects[index2].center.y) / 2+diff.y;
    float meanheight = (possibleRects[index1].size.height + possibleRects[index2].size.height) / 2;
    float xDiff = fabs(possibleRects[index1].center.x - possibleRects[index2].center.x);
    
    finalvertex[0]=Point2f(currentCenter.x-xDiff/2,currentCenter.y-meanheight/2);
    finalvertex[1]=Point2f(currentCenter.x+xDiff/2,currentCenter.y-meanheight/2);
    finalvertex[2]=Point2f(currentCenter.x+xDiff/2,currentCenter.y+meanheight/2);
    finalvertex[3]=Point2f(currentCenter.x-xDiff/2,currentCenter.y+meanheight/2);
    _res_last=Rect(finalvertex[0],finalvertex[2]);
#ifdef LINEARMOR   
    for (int l = 0; l < 4; l++)
			    line(frame, finalvertex[l], finalvertex[(l + 1) % 4], Scalar(0, 0, 250), 2);
#endif
#ifdef SHOW_ARMOR
    imshow("armor",frame);
#endif
    return ;
}